#pragma once

#include <pcl/point_cloud.h>
#include <semaphore.h>

#include <atomic>
#include <chrono>
#include <condition_variable>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <thread>
#include <vector>

#include "client.h"
#include "lidar_scan.h"
#include "point.h"
#include "types.h"

using namespace ouster::sensor;
using namespace ouster;

using namespace std;
// using namespace common;
using Cloud = pcl::PointCloud<ouster_ros::Point>;

struct read_and_cast {
    template <typename T, typename U>
    void operator()(Eigen::Ref<const ouster::img_t<T>> field, ouster::img_t<U>& dest) {
        dest = field.template cast<U>();
    }
};
namespace itage {
namespace modules {
class OusterLidarDriver {
public:
    explicit OusterLidarDriver(const std::string& config_file_str);
    ~OusterLidarDriver();

    // 删除拷贝和移动操作
    OusterLidarDriver(const OusterLidarDriver&) = delete;
    OusterLidarDriver& operator=(const OusterLidarDriver&) = delete;
    OusterLidarDriver(OusterLidarDriver&&) = delete;
    OusterLidarDriver& operator=(OusterLidarDriver&&) = delete;

    bool getPointCloud(Cloud& out_cloud);

private:
    void poll();
    void workerThread();
    void putPointCloud(Cloud&& tmp_cloud);
    void detectTimeout();
    void reconnectClient();

    // 点云处理函数
    void scanToCloud(const ouster::XYZLut& xyz_lut, ouster::LidarScan::ts_t scan_ts, const ouster::LidarScan& ls,
                     Cloud& cloud, int return_index);

    ChanField suitableReturn(sensor::ChanField input_field, bool second);

private:
    // 配置参数
    std::string m_lidarIp;
    std::string m_localIp;

    // 线程控制
    std::atomic<bool> m_running{true};
    std::thread m_workerThread;

    // 点云数据同步
    Cloud m_cloud;
    mutable std::mutex m_cloudMutex;
    std::condition_variable m_cloudReadyCv;
    bool m_cloudReady{false};

    // 网络客户端
    std::shared_ptr<sensor::client> m_client;
    std::mutex m_clientMutex;

    // 状态跟踪
    std::chrono::steady_clock::time_point m_lastDataTime;
    static constexpr std::chrono::milliseconds kTimeout{500};
};
}  // namespace modules
}  // namespace itage
